/* ---<>-----------------------------------
        Includes
---------------------------------------- */
#include <inttypes.h>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include "allocations/daq.h"
#include "util/util.h"
#include "stopWatch.h"
#include "allocations/comPort.h"
#include "global.h"

/* ---<>-----------------------------------
        Namespaces
---------------------------------------- */
using namespace std;
using namespace DAQ_namespace;

extern "C" 
{
/* ---<>-----------------------------------
      StartComPort()
---------------------------------------- */
int StartComPort()
{
	//initialize the com port
	handle = 0;
    return comPortInit(1000000,1000,1000);
}

/* ---<>-----------------------------------
      StartDaq(int frequency, int gains[])
	  
    ADC_CH_GAIN_OFF_gc  = 0,
    ADC_CH_GAIN_1X_gc   = 1, 
    ADC_CH_GAIN_2X_gc   = 2, -10 to 10
    ADC_CH_GAIN_4X_gc   = 3, -5 to 5
    ADC_CH_GAIN_8X_gc   = 4, -2.5 to 2.5
    ADC_CH_GAIN_16X_gc  = 5, -1.25 to 1.25
    ADC_CH_GAIN_32X_gc  = 6	  -0.625 to 0.625
---------------------------------------- */
int StartDaq(int frequency, GAIN_ENUM gains[])
{
	unsigned char packetLength;
	bool success = 0;
	stopWatch timer, timer2;
	timer2.start();
	
	while (timer2.split() < 1)
	{
	//reset of the daq software
	DAQ.sendStopProgram();
	
	DAQ.sendAdcInit(ADC_CH0_bp,gains[0]); //DAQ.sendAdcInit(ADC_CH0_bp,ADC_CH_GAIN_1X_gc);
	DAQ.sendAdcInit(ADC_CH1_bp,gains[1]); //DAQ.sendAdcInit(ADC_CH1_bp,ADC_CH_GAIN_1X_gc);
	DAQ.sendAdcInit(ADC_CH2_bp,gains[2]); //DAQ.sendAdcInit(ADC_CH2_bp,ADC_CH_GAIN_1X_gc);
	DAQ.sendAdcInit(ADC_CH3_bp,gains[3]); //DAQ.sendAdcInit(ADC_CH3_bp,ADC_CH_GAIN_1X_gc);
	
	DAQ.sendDacEnable(DAC_CH0_bp);
	
	//DAQ.sendDigitalInEnable();
	//DAQ.sendEncoderInit(PLOC_D1,1,0,0,DIGFILT_6SAMPLES_gc,1024);
	//DAQ.sendEncoderInit(PLOC_D4,0,1,1,DIGFILT_6SAMPLES_gc,1024);
	//DAQ.sendMotorEnable(0x3);
	//DAQ.sendServoEnable(0xF);
	
	DAQ.sendSetTimerFreq(frequency);
	DAQ.sendStartProgram();	
	
	timer.pause(0.01);
	comPortGet(1,packetLength);// parse then process any incoming messages
	if (DAQ.analogIn[0].enable == 1 && DAQ.analogIn[1].enable == 1 && DAQ.analogIn[2].enable == 1 && DAQ.analogIn[3].enable == 1) 
	{ 
		success = 1; 
		break;
	}
	}
	if (success == 0) { printf("FALIED to open Daq \n"); }
	data.erase(data.begin(),data.end());
	return success;	
}

/* ---<>-----------------------------------
      ReadDaq()
---------------------------------------- */
int ReadDaq(double aIn[], int count)
{
		unsigned char packetLength;
		int returnVal;
		if (count < 2)
		{
			returnVal = comPortGet(0,packetLength);
		}
		else
		{
			returnVal = comPortGet(1,packetLength);
		}
		
		printf("Data available: %d \n",data.size());
		
		if (returnVal != 1) { return -returnVal; }
		
		if (data.size() > 0)
		{
			count = (data.size() >= count) ? count : data.size();
			for (int ii = 0; ii < count; ii ++)
			{
				memcpy( aIn+ii*4, data.front().data(), 4*sizeof(double) );
				data.pop_front();
			}
		}
		else
		{
			return -11;
		}
		return count;
}

/* ---<>-----------------------------------
      StopDaq()
---------------------------------------- */
void StopDaq()
{
	DAQ.sendStopProgram();
	stopWatch timer;
	timer.pause(0.1);
	DAQ.sendStopProgram();
	// Close FTDI Device
	FT_Close(handle);	
}


} // End Extern
